﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Woodstock.Common.Filters
{
    public class ComplementaryFilter
    {
    }


    /******************************************************/
    /*************** Complementary Filter *****************/
    /******************************************************/
    // Originally authored by RoyLB
    // http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286
    public class FlightAngle_CompFilter
    {
        private const int ROLL = 0;
        private const int PITCH = 1;
        private const int YAW = 2;


        float[] previousAngle = new float[2];
        float[] filterTerm0 = new float[2];
        float[] filterTerm1 = new float[2];
        float[] filterTerm2 = new float[2];
        float timeConstantCF;

        float[] angle = new float[3];
        float[] gyroAngle = new float[2];

        float G_Dt = 0.16f; // frequency?


        public bool Initialized { get; set; }


        public void _initialize(int axis, float accelAngleDeg, float gyroRagDegPerSec, float timeConstant) 
        {
            previousAngle[axis] = accelAngleDeg;
            filterTerm2[axis] = gyroRagDegPerSec;
            timeConstantCF = timeConstant; // timeConstant is a global variable read in from EEPROM
            // timeConstantCF should have been read in from set method, but needed common way for CF and KF to be initialized
            // Will take care of better OO implementation in future revision
        }

        public float _calculate(int axis, float newAngle, float newRate) 
        {
            filterTerm0[axis] = (newAngle - previousAngle[axis]) * timeConstantCF *  timeConstantCF;
            filterTerm2[axis] += filterTerm0[axis] * G_Dt;
            filterTerm1[axis] = filterTerm2[axis] + (newAngle - previousAngle[axis]) * 2 *  timeConstantCF + newRate;
            previousAngle[axis] = (filterTerm1[axis] * G_Dt) + previousAngle[axis];
            return previousAngle[axis]; // This is actually the current angle, but is stored for the next iteration
        }


        public FlightAngle_CompFilter()
        {
            filterTerm0[ROLL] = 0;
            filterTerm1[ROLL] = 0;
            filterTerm0[PITCH] = 0;
            filterTerm1[PITCH] = 0;

            Initialized = false;
        }

        public void initialize() 
        {
            for (int axis = ROLL; axis < YAW; axis++)
            {
                //_initialize(axis);
            }
        }

        /*
        void calculate() 
        {
            angle[ROLL] = _calculate(ROLL, accel.angleDeg(ROLL), gyro.rateDegPerSec(ROLL));
            angle[PITCH] = _calculate(PITCH, accel.angleDeg(PITCH), gyro.rateDegPerSec(PITCH));
        }

        float getGyroAngle(byte axis) 
        {
            gyroAngle[axis] += gyro.rateDegPerSec(axis) * G_Dt;
        }*/
    };

}
